#ifndef OBSTACLES_H
#define OBSTACLES_H

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <vector>
#include <utility>
#include <iostream>
#include <cmath>
#include <tf/tf.h>
#include <algorithm> 
#include "ObstaclesManager.h"
#include "match_projection_point.h"
#include "paths_struct.h"


// 定义PI常量
const double PI = 3.14159265358979323846;

// 定义Obstacles类
class Obstacles {
public:
    // 构造函数和析构函数
    Obstacles();
    ~Obstacles();

    // 激光数据处理函数
    void processLaserData(const sensor_msgs::LaserScan& scan);

    // 将障碍物坐标转换到地图坐标
    void transformToMapCoordinates(double x, double y, const std::vector<std::pair<double, double>>& xy_points);

    std::vector<PointAndIndex> getObstaclesProjections();
    std::vector<myObstacle> getObstaclesPoints();

    void laser(const sensor_msgs::LaserScan& scan, const nav_msgs::Odometry& odom, const std::vector<std::pair<double, double>>& xy_points);


    void visualizeObstacles(const std::vector<myObstacle>& point);  // 可视化障碍物

private:
    // 车辆的位置和姿态
    double vehicle_x, vehicle_y, vehicle_yaw;

    // 障碍物坐标列表
    std::vector<std::pair<double, double>> obstacles;
    std::vector<PointAndIndex> obstacles_projections;
    std::vector<std::pair<double, double>> map_coordinates;


    // 激光数据订阅者
    ros::NodeHandle nh;
    ros::Publisher marker_pub_;


    ObstaclesManager obstacles_manager;
    Match_Projection_Point obs_projection;
    PointAndIndex obstacles_projection;

};

#endif // OBSTACLES_H

